/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* C3DCorrelationScanMatch
* This is a "brute force" scan match algorithm that calculates translation and rotation 
* between two scans.The algorithm is based on calculation of 3D correlation (in pose space)
* 
* The correlation criterion is based loosly on discretation of the scans. Basically the match 
* between scan points is approved if the distance between points is less than specified. The
* correlation value is the sum of hits.
*
* Algorithms that could be reused:
* - Heap sort (hpsrt()) Sorts a float vector and returns the sorted array and index that corresponds to original index
* - LSQ Pose estimation (MatchPairPoints). Makes a pose estimate using point pairs.
* 
* @author Jari Saarinen
* @acknoledgements Janne Paanajärvi, Jorma Selkainaho
* 
**/
#include<time.h>
#include"C3DCorrelationScanMatch.h"

/**
* Default constructor
* Sets default parameters
* Assumes: 361 measurement with 0.5 deg... 
*/
  C3DCorrelationScanMatch::C3DCorrelationScanMatch(){
  setNumberOfParticles(8,10,10);
  setGridSize((float)(.6/57.29578),0.06,0.06,0.06);
  numOfMeasurements = 361;
  
  sinlut=(float *) malloc(numOfMeasurements*NNNA*sizeof(float));
  coslut=(float *) malloc(numOfMeasurements*NNNA*sizeof(float));
  ix= (float * ) malloc((numOfMeasurements+1) * sizeof(float));
  iy= (float * ) malloc((numOfMeasurements+1) * sizeof(float));
  ix2=(float * ) malloc((numOfMeasurements+1) * sizeof(float));
  
  fprintf(stderr,"Max search areas x=%.2fm y=%.2fm a=%.2frad\n",NNNX*xdd,NNNY*ydd,NNNA*add);
}
	
/**
 * My own constructor, this is the only thing that can be different from others
 */
C3DCorrelationScanMatch::C3DCorrelationScanMatch(int NX,
                                                int NY,
                                                int NA,
                                                float xd,
                                                float yd,
                                                float ad,
                                                float dd,
                                                int maxNumOfMeasurements){
  
  setNumberOfParticles(NX,NY,NA);
  setGridSize(ad,xd,yd,dd);
  numOfMeasurements = maxNumOfMeasurements;
  
  sinlut=(float *) malloc(numOfMeasurements*NNNA*sizeof(float));
  coslut=(float *) malloc(numOfMeasurements*NNNA*sizeof(float));
  ix= (float * ) malloc((numOfMeasurements+1) * sizeof(float));
  iy= (float * ) malloc((numOfMeasurements+1) * sizeof(float));
  ix2=(float * ) malloc((numOfMeasurements+1) * sizeof(float));
  
  fprintf(stderr,"Max search areas x=%.2fm y=%.2fm a=%.2frad\n",NNNX*xdd,NNNY*ydd,NNNA*add);
  
}

C3DCorrelationScanMatch::~C3DCorrelationScanMatch(){
  free(sinlut);
  free(coslut);
  free(ix);
  free(ix2);
  free(iy);
}
        
/////////////////////////////////////////////
//// SCAN MATCH INTERFACE ///////////////////
////////////////////////////////////////////
/**
 * Updates the position estimate with the new measurement @meas. 
 * @param &meas The latest range scan measurement
 * @param &est  The estimated position, where the measurement is from.
 * @param Goodness The goodness (probability/likelihood) of the match [0...1] 1 is perfect 0 is none 
 */
float C3DCorrelationScanMatch::update(ScanMatch::scan &meas, ScanMatch::pose est){
  float result = 0;
  clock_t t1,t2;
  float t;
  int hit;
  //fprintf(stderr,"Estimating... pos now (%.2f,%.2f,%.2f)",pos.x,pos.y,pos.a);
  t1=clock();
  hit = sickmap(meas, est );
  t2=clock();
  t = (float)(t2-t1)/(float) CLOCKS_PER_SEC;
  fprintf(stderr," pos now (%.2f,%.2f,%.2f) time elapsed:%.2f hits=%d\n",pos.x,pos.y,pos.a,t,hit);
  return result;
}
  
  
  ///////////////////////////////////////////// /////////////////// ///////////////////
  ////////// C3DCorrelationScanMatch - CANNOT BE USED FROM SCANMATCH INTERFACE ///////
  /////////////////////////////////////////////////////////////// ///////////////////
  /**
  * setNumberOfParticles(int NX,int NY, int NA);
  * 
  * Sets the number of particles used in scan matching
  *
  * NX, number of X grids
  * NY, number of Y grids
  * NA, number of angle grids
  **/
void C3DCorrelationScanMatch::setNumberOfParticles(int NX,int NY, int NA){
  NNNX=NX;
  NNNY=NY;
  NNNA=NA;
}
  /**
 *void setGridSize(float ad, float xd, float yd, float dd);
 *
 * sets the grid size for search grids
 *
 * ad - angle resolution in radians
 * xd - X resolution in meters
 * yd - Y resolution in meters
 * dd - longest distance between closest points... if shorter hit is increased
   **/
void C3DCorrelationScanMatch::setGridSize(float ad, float xd, float yd, float dd){
  add=ad;
  xdd=xd;
  ydd=yd;
  ddd=dd;
}

/** 
* Transforms the r,a scan to x,y
* @param  *r  The rangemeasurements
 * @param *a The angle of measurement
 * @param *x The output x-coordinates
 * @param *y The output y-coordinates
 * @param pose The pose where the r,a scan was taken
 * @param inputSize The size of the scan
*/
void C3DCorrelationScanMatch::scan2absoluteXY(float *r, float *a, float *x, float *y, ScanMatch::pose pose, int inputSize){
  int i;
	
  for(i=0;i<inputSize;i++){
    x[i]=r[i]*cos(a[i]+pose.a)+pose.x;
    y[i]=r[i]*sin(a[i]+pose.a)+pose.y;
  }
}

/**
* Integrates the relative movement to new absolute pose
**/
ScanMatch::pose C3DCorrelationScanMatch::integrateNewPositionFromReference(ScanMatch::pose refP, float *v ){
  float R;
  float  Phii;
  ScanMatch::pose newP;

  R=sqrt(v[0]*v[0] + v[1]*v[1]);
  Phii=atan2(v[1],v[0]);

  newP.x = refP.x + R * cos(refP.a+Phii);
  newP.y = refP.y + R * sin(refP.a+Phii);
  newP.a = refP.a + v[2];

  return newP;
}

  
  /**
  * int sickmap(float *a,float *r,float *ix2, float *iy,float *v );
  * 
  * calculates match between reference scan and measured scan
  * 
  * *a - angles corresponding to measurement r
  * *r - measurement
  * *ix2 - x coordinates of reference "map"
  * *iy  - y coordinates of reference "map"
  * *v  - initial position between ref and act scan + the result is returned here
  *  
  * Returns:int hits, number of hits that corresponds to the best match
  **/ 
int C3DCorrelationScanMatch::sickmap(ScanMatch::scan &act_scan, ScanMatch::pose est ){
  int m=0,j,i,n,p,q,I,M,N,indx[1024],ia,ib,ic,numa;
  int hit,hit0;
  float iy0,ix0,ad,xd,yd;
  float LeftLimitForAngle;
  float LeftLimitForX;
  float LeftLimitForY;
  float lsqX=0,lsqY=0,lsqA=0;
  //int counter=0;
  //int smallestHit=0;
  //int smallestHitIndex=0;
  float GyroAngle;
  TFPoint ref_points[361],act_points[361];
  float v[3];
  float *a,*r;
  float distvect[361];
  ScanMatch::pose matched; /// Estimate of the current scan in global coordinates
  //FILE *deb;

  // The initial estimate of the position of the latest scan
  v[0] = est.x;
  v[1] = est.y;
  v[2] = est.a;
	
	
  fprintf(stderr,"rpos:(%.2f %.2f %.2f) est:(%.2f %.2f %.2f)",
	  rpos.x,rpos.y,rpos.a,v[0],v[1],v[2]);
  /// The number of elements in the scans
  M = act_scan.N;
  N = ref.N;
  //fprintf(stderr, " (N=%d,M=%d) ",N,M);
  /// Set the pointers to actual scan (wrapping the old stuff)
  a = act_scan.a;
  r = act_scan.r;
  
  /**fprintf(stderr,"---------------------------------------------\n");
  for(i=0;i<361;i++){
    fprintf(stderr,"%.1f ",act_scan.r[i]);
  }
  fprintf(stderr,"---------------------------------------------\n");**/
  
  //scan2absoluteXY(act_scan.r, act_scan.a, ix2, iy, g_est, N);
  
  //deb = fopen("meas.txt","wt");
  //for(i=0;i<361;i++){
  //  fprintf(deb,"%.2f %.2f ",ix2[i],iy[i]);
 // }
 // fprintf(deb,"\n");
 // fclose(deb);
  
  /// Transform the reference to x,y-space
	ScanMatch::pose zero;
	zero.set(0,0,0);
	scan2absoluteXY(ref.r, ref.a, ix2, iy, zero, N);
  //scan2absoluteXY(ref.r, ref.a, ix2, iy, rpos, N);
  
  //deb = fopen("reference.txt","wt");
  //for(i=0;i<361;i++){
  //  fprintf(deb,"%.2f %.2f ",ix2[i],iy[i]);
  //}
  //fprintf(deb,"\n");
  //fclose(deb);

  GyroAngle=(v[2])*(float)57.0;
  numa=NNNA;
  
  LeftLimitForAngle = v[2]-add*(numa/2);
  LeftLimitForX = v[0]-(NNNX/2)*xdd;
  LeftLimitForY= v[1]-(NNNY/2)*ydd; 
	
  for(m=0;m<N;m++)
  {
    ix[m]=ix2[m];
    indx[m]=m;
  }
  ix[N]=1000000.;
  hpsrt(ix,indx,N);
	
  hit0=0;
   
  /*Cycle through x,y,a ...*/
	
  for(ad=LeftLimitForAngle,I=0,i=0;i<numa;i++,I+=M)
  {
    ad+=add;
    for(j=0;j<M;j++)
    {
      coslut[I+j]=cos(a[j]+ad);
      sinlut[I+j]=sin(a[j]+ad);
    }
  }
	
  for(ad=LeftLimitForAngle,I=0,n=0;n<numa;n++,I+=M)
  {
    ad+=add;
    xd=LeftLimitForX;
    for(p=0;p<NNNX;p++)
    {
      xd+=xdd;
      yd=LeftLimitForY;
      for(q=0;q<NNNY;q++)
      {
        yd+=ydd;
        hit=0;
				
        /*and calculate number of matches;complexity is here M*log2(m) */
        for(j=0;j<M;j++)
        {
				    
          if (r[j]>100.);
          else
          {
            ix0=r[j]*coslut[I+j]+xd-ddd;
            iy0=r[j]*sinlut[I+j]+yd;
						
            ia=-1;
            ib=N;
					
            if(((ix[0]+xdd)>ix0)||(ix0>ix[N-1]));
            else
            {
							
              while((ib-ia)>1)
              {
                ic=(ia+ib)>>1;
                if(ix0<=ix[ic])
                  ib=ic;
                else
                  ia=ic;
              }/*while*/
              if(ia==-1)
                ic=0;
              else
                ic=ia+1;
						        
              ix0=ix0+ddd;
              while((ix[ic]-ix0)<ddd)
              {
                if(fabs(iy0-iy[indx[ic]])<ddd)
                {
                  ref_points[(int)hit].X=ix[ic];
                  ref_points[(int)hit].Y=iy[indx[ic]];
                  act_points[(int)hit].X=ix0;
                  act_points[(int)hit].Y=iy0;
                  hit++;
                  break;
                }
                ic++;
              }
            }/*else*/
          }/*else*/
						
        }/*for j*/
					


        if (hit>hit0)
        {
          if(hit>100){
            MatchPairPoints(ref_points, act_points, (hit-1), &lsqX, &lsqY, &lsqA);
          }
          else{
            lsqA=0;
            lsqX=0;
            lsqY=0;
          }
					matched.set(lsqX,lsqY,lsqA);
					
          v[2]=ad-lsqA;
          v[0]=xd-lsqX;
          v[1]=yd-lsqY;

          hit0=hit;
        }
      }/* q */
			
    }/* p */
		
  } /* n */
	
	ScanMatch::pose tmp_pos;
	tmp_pos.set(v[0],v[1],v[2]);
	fprintf(stderr,"result (%.2f %.2f %.2f) ",v[0],v[1],v[2]);
	pos = rpos.integrateDifferential(tmp_pos);
  
  return hit0;
}
  
          
////////////////////////////////////////////////////////////////////////////////////////
///// Privates
/////////////////////////////////////////////////////////////////////////////////////////
/**
* A Least Squares optimisation based on matched pointpairs
* The pairs should be ordered so that refpoint[i] corresponds to actualpoint[i].
* @param RefPoints[]    List of points in reference scan
* @param ActualPoints[] List of points in the current measurement
* @param N              Number of point pairs
* @param *Tx            @out Translation in x-dir  
* @param *Ty            @out Translation in y-dir
* @param *fi            @out Rotation in heading
  **/
void C3DCorrelationScanMatch::MatchPairPoints(TFPoint RefPoints[], TFPoint ActualPoints[], int N,
    float *Tx, float *Ty, float *fi){
  
    float x=0,y=0,xr=0,yr=0;
    float Sum_xx=0,Sum_yy=0,Sum_xy=0,Sum_yx=0;
    int i;

    if (N<3) {
      fprintf(stderr,"MatchPairPoints::Low number of points\n");
      *Tx=0;
      *Ty=0;
      *fi=0;
      return;
    }

    for(i=0; i<N; i++) {
      xr += RefPoints[i].X;
      yr += RefPoints[i].Y;
      x  += ActualPoints[i].X;
      y  += ActualPoints[i].Y;
    }

    x = x/(float)N;   y = y/(float)N;
    xr= xr/(float)N;  yr= yr/(float)N;

    for(i=0; i<N; i++) {
      Sum_xx += (RefPoints[i].X - xr) * (ActualPoints[i].X - x);
      Sum_yy += (RefPoints[i].Y - yr) * (ActualPoints[i].Y - y);
      Sum_xy += (RefPoints[i].X - xr) * (ActualPoints[i].Y - y);
      Sum_yx += (RefPoints[i].Y - yr) * (ActualPoints[i].X - x);
    }

    *fi = atan((Sum_xy-Sum_yx)/(Sum_xx+Sum_yy));
    *Tx = x - (xr*cos(*fi)-yr*sin(*fi));
    *Ty = y - (xr*sin(*fi)+yr*cos(*fi));
}

/**
* Heap Sort algorithm
* @param *ra pointer to values
* @param *indx The index of values after sort
* @param N The number of values
**/
void C3DCorrelationScanMatch::hpsrt(float *ra,int * indx, int n){
  unsigned long i,ir,j,k,ira;
  float rra;
  
  if (n < 1) return;
  k=(n >> 1);
  ir=n-1;
  for (;;) 
  {
    if(k > 0)
    { 
      rra=ra[--k];
      ira=indx[k];
    }
    else 
    {
      rra=ra[ir];
      ira=indx[ir];
      ra[ir]=ra[0];
      indx[ir]=indx[0];
      if (--ir == 0) 
      {
        ra[0]=rra; 
        indx[0]=ira; 
        break;
      }
    }
    i=k;
    j=k+1;
    while (j <= ir)
    {
      if (j < ir && ra[j] < ra[j+1]) j++;
      if (rra < ra[j])
      {
        ra[i]=ra[j];
        indx[i]=indx[j];  
        i=j;
        j <<= 1;
      } else break;
    }
    ra[i]=rra;
    indx[i]=ira;
  }
}
